#include <iostream>
#include <stdio.h>
using std::cout;
using std::endl;

#include <motion/Matrix.h>
#include <motion/Leg.h>
#include <motion/Robot.h>
#include <motion/Link.h>
#include <motion/Gait.h>
#include <cmath>
#include <motion/XYController.h>
#include <motion/CommunicationMan.h>
#include <motion/Timer.h>

int main() {
	double legx[4];
	double legy[4];
	double legz[4];
	int stop=1;
	while(stop==1){
	int gaitnum;
	double Xpos;
	double Ypos;
	double setspeed;
	//Create a Robot with 4 legs, 3 links per leg.
	cout << "\nWelcome to the Sabertooth program! \n\n" <<
			"\n\t1. Walking Simple \n\t2. Walking Trot \n\t3. Trot \n\t4. Rotary Gallop \n\t5. Traverse Gallop" <<
			"\n\t6. Bound \n\t 0. Debug n Test \n Please input the type of gait you would like to achieve: ";
	
	cin >> gaitnum;
	
	if(gaitnum!=0){
	cout << "X position to go to: ";
	cin >> Xpos;

	cout << "Y position to go to: ";
	cin >> Ypos;

	cout << "What is the speed in meters/sec you'd like to achieve(decimals allowed)? ";
	cin >> setspeed;
	
	}
	else if(gaitnum==0){
	Xpos=0;
	Ypos=1;
	setspeed=.01;
	}
/*
	cout << "H parm : " ;
	double h;
	cin >> h;

	cout << "L parm : ";
	double l;
	cin >> l;*/

	Robot *ST = new Robot();
	Gait *GC = new Gait(ST, gaitnum,setspeed);
	XYController *XY = new XYController(GC);
	char ip[15]="10.0.0.100";
	CommunicationMan *CM = new CommunicationMan(ST,ip,9000);
	Timer *T = new Timer();

	for (int legnum = 0; legnum < LEGS; legnum++) {
		//cout << "Leg Number: " << legnum << endl << endl;

		//Link 1
		XY->gait->getRobot()->legs[legnum].links[0].setLength(LINK1LENGTH);
		XY->gait->getRobot()->legs[legnum].links[0].setMass(LINK1MASS);
		Matrix CG_Link1(1, 4, 0.076, 90.0, 0.0, 90.0);
		XY->gait->getRobot()->legs[legnum].links[0].setCG(CG_Link1);
		Matrix DH_Link1(1, 4, 0.15, 90.0, 0.0, 90.0);
		XY->gait->getRobot()->legs[legnum].links[0].setDH(DH_Link1);

		//Link 2
		XY->gait->getRobot()->legs[legnum].links[1].setLength(LINK2LENGTH);
		XY->gait->getRobot()->legs[legnum].links[1].setMass(LINK2MASS);
		Matrix CG_Link2(1, 4, LINK2LENGTHCG, 0.0, 0.0, 0.0);
		XY->gait->getRobot()->legs[legnum].links[1].setCG(CG_Link2);
		Matrix DH_Link2(1, 4, LINK2LENGTH, 0.0, 0.0, 0.0);
		XY->gait->getRobot()->legs[legnum].links[1].setDH(DH_Link2);

		//Link 3
		XY->gait->getRobot()->legs[legnum].links[2].setLength(LINK3LENGTH);
		XY->gait->getRobot()->legs[legnum].links[2].setMass(LINK3MASS);
		Matrix CG_Link3(1, 4, LINK3LENGTHCG, 0.0, 0.0, 0.0);
		XY->gait->getRobot()->legs[legnum].links[2].setCG(CG_Link3);
		Matrix DH_Link3(1, 4, LINK3LENGTH, 0.0, 0.0, 0.0);
		XY->gait->getRobot()->legs[legnum].links[2].setDH(DH_Link3);

		//Front legs are 0,1
		if((legnum==0)||(legnum==1)){
		XY->gait->getRobot()->legs[legnum].setUnit(1.0);
		}
		else if((legnum==2)||(legnum==3)){
		//Back legs are 2,3
		XY->gait->getRobot()->legs[legnum].setUnit(-1.0);
		}

		//Set Initial XYZ of leg's tips
		XY->gait->getRobot()->legs[legnum].setDesiredx(0);
		double git = XY->gait->getRobot()->legs[legnum].getUnit()*-0.5*XY->gait->getLength();
		XY->gait->getRobot()->legs[legnum].setDesiredy(git);
		XY->gait->getRobot()->legs[legnum].setDesiredz(XY->gait->getLegmax());
	}
	
	XY->setDesiredX(Xpos);
	XY->setDesiredY(Ypos);
	XY->setCurrentX(0.0);
	XY->setCurrentY(0.0);

	int swtemp=0;

	//GC->getRobot()->printInfo();
	//GC->printInfo();

	XY->calculateIterations();
	//swtemp calculates the amount of wait to achieve the proper velocity
	swtemp=XY->gait->getSpeedWait()*1000;	
	
	//Go to position created in the XYController
	for(int i=0; i<XY->getI(); i++){
		cout << i <<endl;

		XY->calculateVectors();

		//Go through the gait.
		for(int o=0; o<LEGS; o++){
			legx[o]=XY->gait->getRobot()->legs[o].getDesiredx();
			legy[o]=XY->gait->getRobot()->legs[o].getDesiredy();
			legz[o]=XY->gait->getRobot()->legs[o].getDesiredz();
		}
		for(int g=1; g<XY->gait->getOffset4(); g++){
			int temp=0;

			XY->gait->setGaitmotion(g); // Set the counter in the Gait

			XY->gait->calculateGait(); // Calculate where the robot should move through the gait

			//Calculate the angles necessary for the robot needs to move to!
			XY->gait->getRobot()->inverseK();

			//GC->getRobot()->printIK();
			//GC->printDesiredPositions(); //Prints desired positions

			if(g>3){
				XY->gait->logDesiredPositions();
				XY->gait->getRobot()->logIK();
			}

			
			while(swtemp>=50){
				swtemp=swtemp-50;
				CM->sendAngles(swtemp);
				T->waitms(50);
			}
			if(swtemp<50){
				swtemp=swtemp+XY->gait->getSpeedWait()*1000;
			}

		}
		for(int o=0; o<LEGS; o++){
			//Set time to 0 at end of the gait, just to be sure.
			XY->gait->getRobot()->legs[o].setDesiredx(legx[o]);
			XY->gait->getRobot()->legs[o].setDesiredy(legy[o]);
			XY->gait->getRobot()->legs[o].setDesiredz(legz[o]);
		}
		CM->sendAngles(50);
	}

	delete XY;
	//cout << "Destroying Robot" << endl;
	delete GC;
	//cout << "Robot Destroyed" << endl;
	//cout << "Destroying Gait Controller" <<endl;
	delete CM;
	delete ST;
	//cout << "Gait Controller Destroyed" << endl;
	delete T;

	cout<< "\nRestart?\n\t1. Yes\n\t2. No" << endl;
	cin >> stop;

	}
	return 0;

}
